
#include "funcdef.h"

/* Sensor Parameter for HMC5883L */
Sensor_Param HMC5883L_Param = {
		0x03,			//ID;
		HMC5883_ADDR,      	//sensor_slave_addr;
		UNINITIALISED,		//UNINITIALISED;
		STANDARD_MODE,		//Opmode mode;
		0,	//max_reg_addr;
		0		//min_reg_addr;
};


const int MAG_SCALE[8] = {1370,1090,820,660,440,390,330,230};

int MAG_CURRENT_MODE = 0;
int MAG_CURRENT_SCALE = 0;
Uint8 MAG_CURRENT_SCALE_INDEX = 0;


int HMC5883L_init(TSK_Ctrl_b * tsk);
static int sync_message_rec(QUE_Handle queue, Msg * message);

/*--------------HMC5883L_sample_tsk()-----------------------------
 * This task is set up to run periodically by pending on HMC5883L_sem,
 * which is update by PRD module.
 *------------------------------------------------ ------*/
void HMC5883L_sample_tsk(void)
{
	short rdata[3];
	double data[4];
	int error = 0;
	Msg message;
	QUE_Handle reply_queue;
	SEM_Handle msg_sem;

	TSK_Ctrl_b *self = &(SCB.TCB[TSK_HMC5883L]);


	SEM_pend(&HMC5883L_sem, SYS_FOREVER);
	// Check if the sensor has been initialized
	while(self->status != INITIALISED){
		error = HMC5883L_init(self);
		//DEBUG
		if(error){
			LOG_printf(self->trace, "HMC5883L INIT failed: %d\n", error);
			error = GIO_control(self->inhd,I2c_IOCTL_DEVICE_SOFT_RESET,NULL);
			if(error){
				LOG_printf(self->trace, "RESET IIC failed: %d\n", error);
			}else{
				LOG_printf(self->trace, "RESET IIC succeed\n");
			}
		}
	}


	vTskTime(self,RESET_TIME);
	/* Initialize task's STS object to current time */
	TSK_settime(TSK_self());
	for(;;){

		SEM_pend(&HMC5883L_sem, SYS_FOREVER);

		//vTskTime(self,SET_PREV_TIME);
		STS_set(&sts_HMC5883L, SCB.TIME_STAMP);
		/* continue to sample the raw data */
		error = HMC5883_GET_RDATA(self->inhd,self->outhd,rdata);
		if(error){
			error = GIO_control(self->inhd,I2c_IOCTL_DEVICE_SOFT_RESET,NULL);
			//DEBUG
			if(error){
				LOG_printf(self->trace, "RESET IIC failed: %d\n", error);
			}else{
				LOG_printf(self->trace, "RESET IIC succeed\n");
			}
			LOG_printf(self->trace, "HMC5883_GETRAWDATA failed: %d\n", error);
		}else{
			vNavData.rMag_x = rdata[0];
			vNavData.rMag_y = rdata[1];
			vNavData.rMag_z = rdata[2];
			HMC5883_SCALE_DATA(rdata,data, vNavData.pitch,vNavData.roll, MAG_CURRENT_SCALE);
			vNavData.Mag_x   = data[0];
			vNavData.Mag_y   = data[1];
			vNavData.Mag_z   = data[2];
			vNavData.azimuth = data[3];
		}

		/* Sensor initialized, perform sampling operations */
		if(sync_message_rec(self->queue, &message)){
			/* receive message from queue */
			if(message->sender >= NUM_TSKS){
				//msg error
				//DEBUG
				LOG_printf(self->trace, "ERROR message from sender: %d\n", message->sender);
			}else{
				if(MAG_SET_MODE_CMD == message->cmd_1){
					error = HMC5883_SET_MODE(self->inhd,self->outhd, message->cmd_2);
				}else if(MAG_SET_RANGE_CMD == message->cmd_1){
					error = HMC5883_SET_RANGE(self->inhd,self->outhd,message->cmd_2);
				}else{
					error = INVALID_CMD;
					//DEBUG
					LOG_printf(self->trace, "Invalid cmd_1 %d : cmd_2 %d\n", message->cmd_1, message->cmd_2);
				}

				if(!error){
					//update magnetometer current setting parameters
					switch(message->cmd_1){
					case MAG_SET_MODE_CMD:
						MAG_CURRENT_MODE = message->cmd_2;
						break;
					case MAG_SET_RANGE_CMD:
						MAG_CURRENT_SCALE_INDEX = message->cmd_2;
						MAG_CURRENT_SCALE = MAG_SCALE[message->cmd_2];
						break;
					}
					//DEBUG
					LOG_printf(self->trace, "TSK_HMC5883l: set mag successfully\n");
				}
				/* Finish and reply to sender */
				message->status = error;
				/* copy the queue semaphore and reply queue*/
				msg_sem = message->sem;
				reply_queue = message->receipt;
				/* put message back onto sender's queue */
				QUE_put(reply_queue, message);
				/* signal the sender on completion */
				SEM_post(msg_sem);
			}
		}
		vTskTime(self,DELTA_TIME);
		STS_delta(&sts_HMC5883L,SCB.TIME_STAMP);
		/* Get time difference and
		add it to task's STS object */
		TSK_deltatime(TSK_self());
		TSK_yield();
	}
}


/* This function receive one small size message from queue synchronously */
static int sync_message_rec(QUE_Handle queue, Msg * message){
	if (QUE_empty(queue)) {
		return 0;
	}
	/* dequeue message */
	*message = QUE_get(queue);
	/* free msg - only done by sender if send in synchronize faction */
	//MEM_free(0, msg, sizeof(MsgObj));
	return 1;
}


/*--------------BMA180_init()---------------------------
 * This function performs deviceID check
 * and read out initial values
 * from BMA180 accelerometer.
 *------------------------------------------------------*/
int HMC5883L_init(TSK_Ctrl_b * tsk){
	int error = 0;
	//set registers
  	error = HMC5883L_WRITE_REG(tsk->inhd,tsk->outhd,HMC5883_R_CONFA, 0x78,VALIDATE);
  	if(error){
  		//DEBUG
  		LOG_printf(tsk->trace, "HMC5883_R_CONFA set failed: %d\n", error);
  		return error;
  	}

	error = HMC5883_SET_RANGE(tsk->inhd,tsk->outhd,MAG_1P3GA);
	if(error){
		//DEBUG
		LOG_printf(tsk->trace, "HMC5883_SETRANGE failed: %d\n", error);
		return error;
	}
	MAG_CURRENT_SCALE_INDEX = MAG_1P3GA;
	MAG_CURRENT_SCALE = MAG_SCALE[MAG_1P3GA];

  	error = HMC5883_SET_MODE(tsk->inhd,tsk->outhd,HMC5883_CONTINMODE);
  	if(error){
  		//DEBUG
  		LOG_printf(tsk->trace, "HMC5883_R_MODE set failed: %d\n", error);
  		return error;
  	}
  	MAG_CURRENT_MODE = HMC5883_CONTINMODE;
  	tsk->status = INITIALISED;
  	//DEBUG
	//tsk->pbuf[]	= "HMC5883L Initialized\n\r";
	//tsk->len	= strlen(tsk->pbuf);
	LOG_printf(tsk->trace, "HMC5883L Initialized Successfully\n");
	return error;
}


